﻿using System;
using System.Diagnostics.Contracts;

namespace OpenLS.Spreadsheet.StandardAddIn
{
    internal class LuDecomposition
    {
        private readonly Matrix _lu;
        private readonly int _pivotSign;
        private readonly int[] _pivotVector;

        public LuDecomposition(Matrix value)
        {
            Contract.Requires(value  != null);
            _lu = value.Clone();
            double[][] lu = _lu.Array;
            int rows = value.RowCount;
            int columns = value.ColumnCount;
            _pivotVector = new int[rows];
            for (int i = 0; i < rows; i++)
            {
                _pivotVector[i] = i;
            }

            _pivotSign = 1;
            double[] lUrowi;
            var LUcolj = new double[rows];

            for (int j = 0; j < columns; j++)
            {
                for (int i = 0; i < rows; i++)
                {
                    LUcolj[i] = lu[i][j];
                }
                for (int i = 0; i < rows; i++)
                {
                    lUrowi = lu[i];
                    int kmax = Math.Min(i, j);
                    double s = 0.0;
                    for (int k = 0; k < kmax; k++)
                    {
                        s += lUrowi[k]*LUcolj[k];
                    }
                    lUrowi[j] = LUcolj[i] -= s;
                }
                int p = j;
                for (int i = j + 1; i < rows; i++)
                {
                    if (Math.Abs(LUcolj[i]) > Math.Abs(LUcolj[p]))
                    {
                        p = i;
                    }
                }

                if (p != j)
                {
                    for (int k = 0; k < columns; k++)
                    {
                        double t = lu[p][k];
                        lu[p][k] = lu[j][k];
                        lu[j][k] = t;
                    }

                    int v = _pivotVector[p];
                    _pivotVector[p] = _pivotVector[j];
                    _pivotVector[j] = v;

                    _pivotSign = -_pivotSign;
                }
                if (j < rows & lu[j][j] != 0.0)
                {
                    for (int i = j + 1; i < rows; i++)
                    {
                        lu[i][j] /= lu[j][j];
                    }
                }
            }
        }

        private bool NonSingular
        {
            get
            {
                for (int j = 0; j < _lu.ColumnCount; j++)
                    if (_lu[j, j] == 0)
                        return false;
                return true;
            }
        }

        public double Determinant
        {
            get
            {
                if (_lu.RowCount != _lu.ColumnCount) throw new ArgumentException("Matrix must be square.");
                double determinant = _pivotSign;
                for (int j = 0; j < _lu.ColumnCount; j++)
                    determinant *= _lu[j, j];
                return determinant;
            }
        }

        public Matrix Solve(Matrix value)
        {
            Contract.Requires(value != null);
            Contract.Assert(value.RowCount == _lu.RowCount);
            //\\Contract.Requires(NonSingular);
            int count = value.ColumnCount;
            Matrix result = value.Submatrix(_pivotVector, 0, count - 1);

            int columns = _lu.ColumnCount;
            double[][] lu = _lu.Array;

            for (int k = 0; k < columns; k++)
            {
                for (int i = k + 1; i < columns; i++)
                {
                    for (int j = 0; j < count; j++)
                    {
                        result[i, j] -= result[k, j]*lu[i][k];
                    }
                }
            }
            for (int k = columns - 1; k >= 0; k--)
            {
                for (int j = 0; j < count; j++)
                {
                    result[k, j] /= lu[k][k];
                }

                for (int i = 0; i < k; i++)
                {
                    for (int j = 0; j < count; j++)
                    {
                        result[i, j] -= result[k, j]*lu[i][k];
                    }
                }
            }
            return result;
        }
    }
}